#include "hapticdv.h"
#include <iostream>

double Maxfocefactor = 0.8;

HapticDV::HapticDV()
{
    dev_id_ = -2; // we set a value not in API defined range
    device_enabled_ = false;

    set_force_ = true;

    for (int i = 0; i<3;i++) {
        position_[i] = 0.0;
    }

    //button0_state_ = false;
    keep_alive_ = false;
    force_released_ = true;

    force_.resize(3);
    force_[0] = 0.0;
    force_[1] = 0.0;
    force_[2] = 0.0;

    SetForceLimit(10.0, 10.0, 10.0);

    GetsdkVesion();

    // connect to hardware device
    device_count_ = dhdGetDeviceCount();

    // we only accept one haptic device.
    if ( device_count_ >= 1) {
        dev_id_ = dhdOpenID(0); // if open failed, we will get -1, and sucess with 0.
        if ( dev_id_ < 0) {
            std::cout<<"error: handler device:"<< dhdErrorGetLastStr()<<std::endl;
            device_enabled_ = false;
            return;
        }
    } else {
        std::cout<<"No handler device find! "<< dhdErrorGetLastStr()<<std::endl;
        device_enabled_ = false;
        return;
    }

    std::cout<<dhdGetSystemName()<<" device detected\n" <<std::endl;
    dhdEnableForce (set_force_);
    device_enabled_ =true;
}

HapticDV::~HapticDV()
{
    dev_id_=-1;
    device_count_=0;
    keep_alive_=false;
    dhdClose();
}

QString HapticDV::GetsdkVesion()
{
    QString str;
    int major,minor,release,revision;
    dhdGetSDKVersion(&major,&minor,&release,&revision);
    str= QString("Force Dimension - SDK %1.%2.%3.%4\n").arg(QString::number(major)).arg(QString::number(minor)).
            arg(QString::number(release)).arg(QString::number( revision));
    str.append("Copyright (C) 2001-2020 Force Dimension\n");
    str.append("2021-09-30\n");
    std::cout<<str.toStdString()<<std::endl;
    return str;
}

void HapticDV::SetForceLimit(double x, double y, double z)
{
    force_x_limit_=x;
    force_y_limit_=y;
    force_z_limit_=z;
}

std::vector<double> HapticDV::GetHapticData()
{

    double feed_force[3] = {0.0, 0.0, 0.0};
    double px=0,py=0,pz=0;
    double rx=0,ry=0,rz=0;
    std::vector<double> vec;
    if (device_count_ >= 1 && dev_id_ >= 0)
    {
        int dt = dhdGetPositionAndOrientationRad(&px, &py, &pz,&rx,&ry,&rz);
        if(dt!=0)
            return {};
        //std::cout<<" dhdGetPositionAndOrientationRad "<<dt<<std::endl;
        vec.push_back(px);
        vec.push_back(py);
        vec.push_back(pz);
        vec.push_back(rx);
        vec.push_back(ry);
        vec.push_back(rz);
        dhdGetGripperAngleRad(&gripper_angle_);
    }

    if (set_force_) {
        val_lock_.lock();
        feed_force[0] = force_[0];
        feed_force[1] = force_[1];
        feed_force[2] = force_[2];
        dhdSetForce(feed_force[0], feed_force[1], feed_force[2]);
        val_lock_.unlock();
    }
    return vec;
}

std::vector<double> HapticDV::GetHapticSpeed()
{
    double spx,spy,spz,rpx,rpy,rpz;
    std::vector<double> speed;
    if(device_count_>=1 && dev_id_ >= 0)
    {
        int dt= dhdGetLinearVelocity(&spx,&spy,&spz);
        if(dt!=0)
            return {};
        dhdGetAngularVelocityRad(&rpx,&rpy,&rpz);
        speed.push_back(qAbs(spx)<0.001?0:spx);
        speed.push_back(qAbs(spy)<0.001?0:spy);
        speed.push_back(qAbs(spz)<0.001?0:spz);
        speed.push_back(qAbs(rpx)<0.001?0:rpx);
        speed.push_back(qAbs(rpy)<0.001?0:rpy);
        speed.push_back(qAbs(rpz)<0.001?0:rpz);
    }
    return speed;
}

std::vector<double> HapticDV::GetHapticForce()
{
    double fx,fy,fz,tx,ty,tz;
    std::vector<double> force;
    if(device_count_>=1 && dev_id_ >= 0)
    {
        int dt = dhdGetForceAndTorque(&fx,&fy,&fz,&tx,&ty,&tz);
        if(dt!=0)
            return {};
        force.push_back(qAbs(fx)<1e-3?0:fx);
        force.push_back(qAbs(fy)<1e-3?0:fy);
        force.push_back(qAbs(fz)<1e-3?0:fz);
        force.push_back(qAbs(tx)<1e-3?0:tx);
        force.push_back(qAbs(ty)<1e-3?0:ty);
        force.push_back(qAbs(tz)<1e-3?0:tz);
    }
    return force;
}

bool HapticDV::setHapticForce(std::vector<double> vec)
{
    if(vec.size()<6)
    {
        std::cout<<"size error"<<std::endl;
        return false;
    }
    std::vector<double> fvec;
    for(int i=0;i<vec.size();++i)
    {
        fvec.push_back(vec.at(i)-zeroforce_.at(i));
    }

    double fac= Maxfocefactor*forcefactor_;
    double fx=fvec.at(0)*fac,fy=fvec.at(1)*fac,fz=fvec.at(2)*fac,tx=fvec.at(3)*fac,ty=fvec.at(4)*fac,tz=fvec.at(5)*fac;
    //std::cout<<"haptic force "<<fx<<" "<<fy<<" "<<fz<<" "<<tx<<" "<<ty<<" "<<tz<<std::endl;
    if(device_count_>=1 && dev_id_ >= 0)
    {
        int dt= dhdSetForceAndTorque(fx,fy,fz,tx,ty,tz,dev_id_);
        if(dt!=0)
        {
            std::cout<<"dhdSetForceAndTorque return a value "<<dt<<std::endl;
            return false;
        }
        return true;
    }
    else
        std::cout<<"device lost"<<std::endl;
    return false;
}

void HapticDV::setforcefactor(double fact)
{
    forcefactor_=fact;
}

void HapticDV::setZeroForce(std::vector<double> vec)
{
    std::cout<<"calibration zero points ";
    for(int i=0;i<vec.size();++i)
    {
        std::cout<<" "<<vec.at(i);
    }
    std::cout<<std::endl;
    zeroforce_=vec;
}

void HapticDV::setMaxfactor(double fact)
{
    Maxfocefactor=fact;
}

void HapticDV::SetForce(double x, double y, double z)
{
    double input_force[3] = {0.0, 0.0, 0.0};

    if (set_force_)
    {
        val_lock_.lock();
        input_force[0] = x;
        input_force[1] = y;
        input_force[2] = z;
        VerifyForceLimit(input_force, force_);
        force_released_ = false;
        val_lock_.unlock();
    }
}

void HapticDV::VerifyForceLimit(double input_force[], std::vector<double> & output)
{
    if (output.size() != 3) {
        output.resize(3);
    }

    if (input_force[0] < -force_x_limit_) output[0] = -force_x_limit_;
    if (input_force[1] < -force_y_limit_) output[1] = -force_y_limit_;
    if (input_force[2] < -force_z_limit_) output[2] = -force_z_limit_;

    if (input_force[0] > force_x_limit_) output[0] = force_x_limit_;
    if (input_force[1] > force_y_limit_) output[1] = force_y_limit_;
    if (input_force[2] > force_z_limit_) output[2] = force_z_limit_;
}
